/* -*- mode: asm; coding:utf-8 -*- */
/************************************************************************/
/*  OS kernel sample                                                    */
/*  Copyright 2014 Takeharu KATO                                        */
/*                                                                      */
/*  Exception vector                                                    */
/*                                                                      */
/************************************************************************/
#define ASM_FILE   1

#include "exception.h"
#include "sdkconfig.h"

.section ".text.boot"


.globl _boot
.globl _vector_table
.globl _freertos_vector_table


.macro saveregister
	stp		X0,X1, [sp,#-0x10]!
	stp		X2,X3, [sp,#-0x10]!
	stp		X4,X5, [sp,#-0x10]!
	stp		X6,X7, [sp,#-0x10]!
	stp		X8,X9, [sp,#-0x10]!
	stp		X10,X11, [sp,#-0x10]!
	stp		X12,X13, [sp,#-0x10]!
	stp		X14,X15, [sp,#-0x10]!
	stp		X16,X17, [sp,#-0x10]!
	stp 	X18,X19, [sp,#-0x10]!
	stp 	X29,X30, [sp,#-0x10]!

	mrs x0, CPACR_EL1
	mrs x1, ELR_EL1
	mrs	x2, SPSR_EL1
	stp	x0, x1, [sp,#-0x10]!
	str	x2, [sp,#-0x10]!
.endm

.macro savefloatregister

/* Save all the floating point register to the stack */
	stp	q0,q1, [sp, #-0x20]!
	stp	q2,q3, [sp, #-0x20]!
	stp	q4,q5, [sp, #-0x20]!
	stp	q6,q7, [sp, #-0x20]!
	stp	q8,q9, [sp, #-0x20]!
	stp	q10,q11, [sp, #-0x20]!
	stp	q12,q13, [sp, #-0x20]!
	stp	q14,q15, [sp, #-0x20]!
	stp	q16,q17, [sp, #-0x20]!
	stp	q18,q19, [sp, #-0x20]!
	stp	q20,q21, [sp, #-0x20]!
	stp	q22,q23, [sp, #-0x20]!
	stp	q24,q25, [sp, #-0x20]!
	stp	q26,q27, [sp, #-0x20]!
	stp	q28,q29, [sp, #-0x20]!
	stp	q30,q31, [sp, #-0x20]! 
	mrs	x2, FPCR
	mrs	x3, FPSR
	stp	x2, x3, [sp, #-0x10]!
.endm

.macro restorefloatregister

/* Restore all the floating point register from the stack */
	ldp	x2, x3, [sp] ,#0x10
	msr	FPCR, x2
	msr	FPSR, x3 

	ldp	q30,q31, [sp] ,#0x20
	ldp	q28,q29, [sp] ,#0x20
	ldp	q26,q27, [sp] ,#0x20
	ldp	q24,q25, [sp] ,#0x20
	ldp	q22,q23, [sp] ,#0x20
	ldp	q20,q21, [sp] ,#0x20
	ldp	q18,q19, [sp] ,#0x20
	ldp	q16,q17, [sp] ,#0x20
	ldp	q14,q15, [sp] ,#0x20
	ldp	q12,q13, [sp] ,#0x20
	ldp	q10,q11, [sp] ,#0x20
	ldp	q8,q9, [sp] ,#0x20
	ldp	q6,q7, [sp] ,#0x20
	ldp	q4,q5, [sp] ,#0x20
	ldp	q2,q3, [sp] ,#0x20
	ldp	q0,q1, [sp] ,#0x20
.endm

.macro build_trapframe exc_type
	/*
	 * store generic registers from (x29,x30) pair to (x1,x2) pair.
	 */
	stp	x29, x30, [sp, #-16]!
	stp	x27, x28, [sp, #-16]!
	stp	x25, x26, [sp, #-16]!
	stp	x23, x24, [sp, #-16]!
	stp	x21, x22, [sp, #-16]!
	stp	x19, x20, [sp, #-16]!
	stp	x17, x18, [sp, #-16]!
	stp	x15, x16, [sp, #-16]!
	stp	x13, x14, [sp, #-16]!
	stp	x11, x12, [sp, #-16]!
	stp	x9, x10, [sp, #-16]!
	stp	x7, x8, [sp, #-16]!
	stp	x5, x6, [sp, #-16]!
	stp	x3, x4, [sp, #-16]!
	stp	x1, x2, [sp, #-16]!

	savefloatregister

	/*
	 * Store (spsr, x0)
	 */
	mrs	x21, spsr_el1
	stp	x21, x0, [sp, #-16]!
	/*
	 * Allocate a room for sp_el0 and store elr
	 */
	mrs	x21, elr_el1
	stp	xzr, x21, [sp, #-16]!
	/*
	 * store exception type and esr
	 */
	mov	x21, #(\exc_type)
	mrs	x22, esr_el1
	stp	x21, x22, [sp, #-16]!
.endm

.macro store_traped_sp
	mrs	x21, sp_el0
	str	x21, [sp, #EXC_EXC_SP_OFFSET]
.endm



.macro call_common_trap_handler
	mov	x0, sp
	bl CommonTrapHandler
.endm

.macro store_nested_sp
	mov	x21, sp 	
	add	x21, x21, #EXC_FRAME_SIZE
	str	x21, [sp, #EXC_EXC_SP_OFFSET]
.endm

.macro restore_traped_sp
	ldr	x21, [sp, #EXC_EXC_SP_OFFSET]
	msr	sp_el0, x21
.endm

.macro restore_trapframe

	/*
	 * Drop exception type, esr, 
	 */
	add	sp, sp, #16
	/*
	 * Drop exception stack pointer and restore elr_el1
	 */
	ldp	x21, x22, [sp], #16	
	msr	elr_el1, x22

	/*
	 * Retore spsr and x0
	 */
	ldp	x21, x0, [sp], #16
	msr	spsr_el1, x21

	restorefloatregister

	/*
	 * Restore generic registers from (x29,x30) pair to (x1,x2) pair.
	 */
	ldp	x1, x2, [sp], #16
	ldp	x3, x4, [sp], #16
	ldp	x5, x6, [sp], #16
	ldp	x7, x8, [sp], #16
	ldp	x9, x10, [sp], #16
	ldp	x11, x12, [sp], #16
	ldp	x13, x14, [sp], #16
	ldp	x15, x16, [sp], #16
	ldp	x17, x18, [sp], #16
	ldp	x19, x20, [sp], #16
	ldp	x21, x22, [sp], #16
	ldp	x23, x24, [sp], #16
	ldp	x25, x26, [sp], #16
	ldp	x27, x28, [sp], #16
	ldp	x29, x30, [sp], #16

	eret
.endm


/*
	* Exception vectors.
*/
.balign 2048
_vector_table:

.set	VBAR, _vector_table


/*
	 * Current EL with SP0
	 */
.org VBAR
	/* b	_boot */
	b	_curr_el_sp0_sync		/* Synchronous */
.org (VBAR + 0x80)
	b	_curr_el_sp0_irq		/* IRQ/vIRQ */
.org (VBAR + 0x100)
	b	_curr_el_sp0_fiq		/* FIQ/vFIQ */
.org (VBAR + 0x180)
	b	_curr_el_sp0_serror		/* SError/vSError */

	/*
	 * Current EL with SPx
	 */
.org (VBAR + 0x200)
	b	_curr_el_spx_sync		/* Synchronous */
.org (VBAR + 0x280)
	b	_curr_el_spx_irq		/* IRQ/vIRQ */
.org (VBAR + 0x300)
	b	_curr_el_spx_fiq		/* FIQ/vFIQ */
.org (VBAR + 0x380)
	b	_curr_el_spx_serror		/* SError/vSError */

	/*
	 * Lower EL using AArch64
	 */
.org (VBAR + 0x400)
	b	_lower_el_aarch64_sync
.org (VBAR + 0x480)
	b	_lower_el_aarch64_irq
.org (VBAR + 0x500)
	b	_lower_el_aarch64_fiq
.org (VBAR + 0x580)
	b	_lower_el_aarch64_serror

	/*
	 * Lower EL using AArch32
	 */
.org (VBAR + 0x600)
	b	_lower_el_aarch32_sync
.org (VBAR + 0x680)
	b	_lower_el_aarch32_irq
.org (VBAR + 0x700)
	b	_lower_el_aarch32_fiq
.org (VBAR + 0x780)
	b	_lower_el_aarch32_serror

	text_align
_curr_el_sp0_sync:
	build_trapframe AARCH64_EXC_SYNC_SP0
	store_traped_sp
	call_common_trap_handler
	restore_traped_sp
	restore_trapframe

	text_align
_curr_el_sp0_irq:
	build_trapframe AARCH64_EXC_IRQ_SP0
	store_traped_sp
	call_common_trap_handler
	restore_traped_sp
	restore_trapframe

	text_align
_curr_el_sp0_fiq:
	build_trapframe AARCH64_EXC_FIQ_SP0
	store_traped_sp
	call_common_trap_handler
	restore_traped_sp
	restore_trapframe

	text_align
_curr_el_sp0_serror:
	build_trapframe AARCH64_EXC_SERR_SP0
	store_traped_sp
	call_common_trap_handler
	restore_traped_sp
	restore_trapframe


	text_align
_curr_el_spx_sync:
	build_trapframe AARCH64_EXC_SYNC_SPX
	store_nested_sp
	call_common_trap_handler
	restore_trapframe

	text_align
_curr_el_spx_irq:
	build_trapframe AARCH64_EXC_IRQ_SPX
	store_nested_sp
	call_common_trap_handler
	restore_trapframe

	text_align
_curr_el_spx_fiq:
	build_trapframe AARCH64_EXC_FIQ_SPX
	store_nested_sp
	call_common_trap_handler
	restore_trapframe

	text_align
_curr_el_spx_serror:
	build_trapframe AARCH64_EXC_SERR_SPX
	store_nested_sp
	call_common_trap_handler
	restore_trapframe


	text_align
_lower_el_aarch64_sync:
	build_trapframe AARCH64_EXC_SYNC_AARCH64
	store_traped_sp
	call_common_trap_handler
	restore_traped_sp
	restore_trapframe

	text_align
_lower_el_aarch64_irq:
	build_trapframe AARCH64_EXC_IRQ_AARCH64
	store_traped_sp
	call_common_trap_handler
	restore_traped_sp
	restore_trapframe

	text_align
_lower_el_aarch64_fiq:
	build_trapframe AARCH64_EXC_FIQ_AARCH64
	store_traped_sp
	call_common_trap_handler
	restore_traped_sp
	restore_trapframe

	text_align
_lower_el_aarch64_serror:
	build_trapframe AARCH64_EXC_SERR_AARCH64
	store_traped_sp
	call_common_trap_handler
	restore_traped_sp
	restore_trapframe


	text_align
_lower_el_aarch32_sync:
	build_trapframe AARCH64_EXC_SYNC_AARCH32
	store_traped_sp
	call_common_trap_handler
	restore_traped_sp
	restore_trapframe

	text_align
_lower_el_aarch32_irq:
	build_trapframe AARCH64_EXC_IRQ_AARCH32
	store_traped_sp
	call_common_trap_handler
	restore_traped_sp
	restore_trapframe

	text_align
_lower_el_aarch32_fiq:
	build_trapframe AARCH64_EXC_FIQ_AARCH32
	store_traped_sp
	call_common_trap_handler
	restore_traped_sp
	restore_trapframe

	text_align
_lower_el_aarch32_serror:
	build_trapframe AARCH64_EXC_SERR_AARCH32
	store_traped_sp
	call_common_trap_handler
	restore_traped_sp
	restore_trapframe


/******************************************************************************
 * Vector table to use when FreeRTOS is running.
 *****************************************************************************/

#ifdef CONFIG_USE_FREERTOS

.set	FREERTOS_VBAR, (VBAR+0x2000)

.org(FREERTOS_VBAR)
_freertos_vector_table:
	b	FreeRTOS_SWI_Handler

.org (FREERTOS_VBAR + 0x80)
	b	FreeRTOS_IRQ_Handler

.org (FREERTOS_VBAR + 0x100)
	b	.

.org (FREERTOS_VBAR + 0x180)
	b	.

.org (FREERTOS_VBAR + 0x200)
	b	FreeRTOS_SWI_Handler

.org (FREERTOS_VBAR + 0x280)
	b	FreeRTOS_IRQ_Handler

.org (FREERTOS_VBAR + 0x300)
	b	.

.org (FREERTOS_VBAR + 0x380)
	b	_curr_el_spx_serror		/* SError/vSError */

.org (FREERTOS_VBAR + 0x400)
	b	.

.org (FREERTOS_VBAR + 0x480)
	b	.

.org (FREERTOS_VBAR + 0x500)
	b	.

.org (FREERTOS_VBAR + 0x580)
	b	.

.org (FREERTOS_VBAR + 0x600)
	b	.

.org (FREERTOS_VBAR + 0x680)
	b	.

.org (FREERTOS_VBAR + 0x700)
	b	.

.org (FREERTOS_VBAR + 0x780)
	b	.

.org (FREERTOS_VBAR + 0x800)


#endif

.end
